#include "intersectiontests.h"

using namespace TorPhysix;

bool IntersectionTests::sphereAndHalfSpace(const CollisionSphere &sphere, const CollisionPlane &plane)
{
	// Find the distance from the origin
	real ballDistance =plane.direction.dot( sphere.getAxis(3) ) - sphere.radius;
	// Check for the intersection
	return ballDistance <= plane.offset;
}

bool IntersectionTests::sphereAndSphere(const CollisionSphere &one, const CollisionSphere &two)
{
	// Find the vector between the objects
	Vec3f midline = one.getAxis(3) - two.getAxis(3);
	// See if it is large enough.
	return length_2(midline) < (one.radius+two.radius)*(one.radius+two.radius);
}

static inline real transformToAxis(const CollisionBox &box,const Vec3f &axis)
{
	return box.halfSize.x() * real_abs( axis.dot(box.getAxis(0)) ) +
		box.halfSize.y() * real_abs( axis.dot(box.getAxis(1)) ) +
		box.halfSize.z() * real_abs( axis.dot(box.getAxis(2)) );
}

bool IntersectionTests::boxAndHalfSpace(const CollisionBox &box,const CollisionPlane &plane)
{
	// Work out the projected radius of the box onto the plane direction
	real projectedRadius = transformToAxis(box, plane.direction);
	// Work out how far the box is from the origin
	real boxDistance = plane.direction.dot(box.getAxis(3)) - projectedRadius;
	// Check for the intersection
	return boxDistance <= plane.offset;
}